//
// Created by liz on 24-11-28.
//

#include "arm.h"
#include "arm_math.h"
#include "user_lib.h"

#include <cmsis_os2.h>
#include <general_def.h>
#include "message_center.h"
#include "message.h"

#include "he_serial_servo.h"
#include "bsp_usart.h"
#include "dji_motor.h"

static Publisher_t *arm_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *arm_sub;                  // cmd控制消息订阅者

Arm_Ctrl_Cmd_s arm_cmd_recv={
    .yaw = 0.0f,
    .height = 13.0f,
    .length = 15.0f,
    .open_angle = 0,
};

static DJIMotorInstance *yaw_motor;

static ServoInstance *servo1, *servo2, *servo3, *servo4, *servo5, *servo6;

float arm_offset[6]={0,220,75,129,0,0};
float arm_reduction[6]={1,1,1,3,1,1};

void Arm_Init(void) {
    Servo_Init_Config_s he_servo_config = {
        .servo_id = 1,
        .usart_handle = &huart3,
        .servo_type = SERVO_BUS,
    };
    servo1 = HE_ServoInit(&he_servo_config);
    he_servo_config.servo_id = 2;
    servo2 = HE_ServoInit(&he_servo_config);
    he_servo_config.servo_id = 3;
    servo3 = HE_ServoInit(&he_servo_config);
    he_servo_config.servo_id = 4;
    servo4 = HE_ServoInit(&he_servo_config);
    he_servo_config.servo_id = 5;
    servo5 = HE_ServoInit(&he_servo_config);

    Servo_Init_Config_s servo_config = {
        .servo_id = 6,
        .pwm_init_config = {
            .tim_handle = &htim2,
            .channel = TIM_CHANNEL_1,
        },
        .angle_min = 0,
        .angle_max = 270,
        .servo_type = SERVO_PWM,
    };
    servo6 = HE_ServoInit(&servo_config);
    // HE_ServoSetAngle(servo6,0,0,0);


    //arm_pub = PubRegister("arm_feed", sizeof(Gimbal_Upload_Data_s));
    arm_sub = SubRegister("arm_cmd", sizeof(Arm_Ctrl_Cmd_s));
}

float target_angle[7]={0,0.0,60.0,120.0,0,0};//机械臂目标角度
float current_angle[7]={0};
float movetime=0;
float movespeed[7]={3,4,2,4,2,2,1};        //一度多少ms
void Arm_CheckMove() {
    for(int i=0;i<7;i++) {
        ServoInstance *servo=NULL;
        switch (i)
        {
        case 0:
            break;
        case 1:
            servo=servo1;
            break;
        case 2:servo=servo2;
            break;
        case 3:servo=servo3;
            break;
        case 4:servo=servo4;
            break;
        case 5:servo=servo5;
            break;
        case 6:
            servo=servo6;
            break;
        default:
            break;
        }

        if(servo!=NULL)
        {
            if(target_angle[i] != current_angle[i]) {
                movetime=fabsf((target_angle[i]-current_angle[i])*movespeed[i]);
                current_angle[i] = target_angle[i];
                if(i!=0)
                {
                    HE_ServoSetAngle(servo,current_angle[i],(uint16_t)movetime,0);
                }
            }
            else
            {
                if(servo->servo_type == SERVO_PWM)
                {
                    HE_ServoSetAngle(servo,current_angle[i],0,0);
                }
            }
            osDelay(2);
        }


    }

}

//机械臂构型：角度、高度、长度、开合程度

#define joint1_a  9.0f
#define joint1_a2 81.0f
#define joint1_c  13.5f
#define joint1_c2 182.25f
#define joint1_d  0.2f
#define joint1_d2 0.04f

#define joint2_a 18.0f
#define joint2_a2 324.0f
#define joint2_b 16.5f
#define joint2_b2 272.25f

#define joint3_close 140
// #define joint4_close 15

#define joint3_open 220
// #define joint4_open 55


// @todo:当前实现只针对末端夹爪，不是正常机械臂解算
void Arm_ConvertAngle() {
    // 限幅
    arm_cmd_recv.height=float_constrain(arm_cmd_recv.height,4.5,18.5);
    arm_cmd_recv.length=float_constrain(arm_cmd_recv.length,12.0,29.5);
    // arm_cmd_recv.camera_angle=float_constrain(arm_cmd_recv.camera_angle,65.0,160.0);

    float joint1length=36.5-10.5-arm_cmd_recv.height;
    float joint2length= (arm_cmd_recv.length + 2.0)* 10.0 ;      //转毫米

    target_angle[1]=-arm_cmd_recv.yaw;

    //angle=acos((joint1_b2-joint1_a2+pow(arm_cmd_recv.height,2))/(2*joint1_b*arm_cmd_recv.height))*RAD_2_DEGREE;
    // target_angle[2]=180.0-acos((joint1_b2-joint1_a2+pow(joint1length,2))/(2*joint1_b*joint1length))*RAD_2_DEGREE;
    target_angle[2]=(acosf((joint1_a2+powf(joint1length,2)+joint1_d2-joint1_c2)/
        (2*joint1_a*sqrtf(powf(joint1length,2)+joint1_d2)))-atanf(joint1_d/joint1length))*RAD_2_DEGREE;
    // y = -9E-12x6 + 1E-08x5 - 5E-06x4 + 0.0013x3 - 0.1776x2 + 12.979x - 342.53
    // target_angle[3] = -(-9E-12 * powf(joint2length,6) + 1E-08 * powf(joint2length,5) + -5E-06 * powf(joint2length,4)
    // + 0.0013 * powf(joint2length,3) + -0.1776*powf(joint2length,2) + 12.979*joint2length - 342.53) ;
    // f: y=-6.833120135934763*10^(-10) x^(5)+6.388771356391487*10^(-7) x^(4)-0.00023437102901624485 x^(3)+0.04140449733586138 x^(2)-3.587043378385715 x+167.0967413128974
    target_angle[3] = -1*(-6.833120135934763E-10 * powf(joint2length, 5) + 6.388771356391487E-7 * powf(joint2length, 4)
     - 0.00023437102901624485 * powf(joint2length, 3) + 0.04140449733586138 * powf(joint2length, 2)
     - 3.587043378385715 * joint2length + 167.0967413128974);

    // target_angle[3]=acos((joint2_b2-joint2_a2+pow(joint2length,2))/(2*joint2_b*joint2length))*RAD_2_DEGREE;

    // if(arm_cmd_recv.open_angle==0)
    // {
    //     target_angle[4]=joint3_close;
    //     target_angle[5]=joint4_close;
    // }
    // else
    // {
    //     target_angle[4]=joint3_open;
    //     target_angle[5]=joint4_open;
    // }
    if(arm_cmd_recv.open_angle==0)
    {
        target_angle[4]=joint3_close;
    }
    else
    {
        target_angle[4]=joint3_open;
    }

    target_angle[6] = arm_cmd_recv.camera_angle;
}

void Arm_CorrectOffset() {
    for(int i=0;i<6;i++) {
        target_angle[i]*=arm_reduction[i];
        target_angle[i]+=arm_offset[i];
    }
}

void Arm_Task(void) {
    SubGetMessage(arm_sub, &arm_cmd_recv);
    Arm_ConvertAngle();
    Arm_CorrectOffset();
    Arm_CheckMove();
    // HE_ServoSetAngle(servo1,target_angle[0],0,0);
    // // osDelay(1);
    // HE_ServoSetAngle(servo2,target_angle[1],0,0);
    // // // osDelay(1);
    // HE_ServoSetAngle(servo3,target_angle[2],0,0);
    // // // osDelay(1);
    // HE_ServoSetAngle(servo4,target_angle[3],0,0);
    // osDelay(1);
    // HE_ServoRead(servo2,READ_POS);
    //DJIMotorSetRef(yaw_motor, target_angle[4]);
}
